%% 根据车辆结束范围搜索停车位的最佳转向角
function [bestEnd,bestEnd1,maximalObstDistance] = SearchSteer(End,Vehicle,Configure)
    % 初始化无穷远距离
    infDistance = 4*sqrt(Configure.XLENGTH^2+Configure.YLENGTH^2);

    % 搜索最佳转向角
    bestEnd = [0,0,0];
    bestEnd1 = [0,0,0];
    maximalObstDistance = -inf;
    % 获取搜索的位置
    angleSample = linspace(Configure.MINYAW,Configure.MAXYAW,Configure.N_ANGLE_SEARCH);
    lengthSample = linspace(0,Configure.MAX_LENGTH_SEARCH,Configure.N_LENGTH_SEARCH);
    for i = 1:length(lengthSample)
        posLength = lengthSample(i);
        if posLength == 0 % 长度为0，不需要角度取样
            pos = [End(1),End(2)];
            % 对所有角度进行搜索
            [bestEndYaw,obstDistance] = SearchSteer1(pos,Vehicle,Configure,infDistance);
            if obstDistance > maximalObstDistance
                maximalObstDistance = obstDistance;
                bestEnd = [pos,bestEndYaw];
                bestEnd1 = [pos,Mod2Pi(bestEndYaw+pi)];
            end
        else
            for j = 1:length(angleSample)
                % 获取新的取样位置
                pos = [End(1)+posLength*cos(angleSample(j)),End(2)+posLength*sin(angleSample(j))];
                % 对所有角度进行搜索
                [bestEndYaw,obstDistance] = SearchSteer1(pos,Vehicle,Configure,infDistance);
                if obstDistance > maximalObstDistance
                    maximalObstDistance = obstDistance;
                    bestEnd = [pos,bestEndYaw];
                    bestEnd1 = [pos,Mod2Pi(bestEndYaw+pi)];
                end
            end
        end
    end
end

%% 根据车辆结束位置搜索停车位的最佳转向角
function [bestEndYaw,maximalObstDistance] = SearchSteer1(Pos,Vehicle,Configure,infDistance)
    bestEndYaw = inf;
    maximalObstDistance = -inf;
    steerSample = linspace(Configure.MINYAW,Configure.MAXYAW,Configure.N_STEER_SEARCH);
    for i = 1:length(steerSample)
        endYaw = steerSample(i);

        % 车辆姿势碰撞检测
        isCollision = VehiclePositionCheck(Pos, endYaw, Vehicle, Configure);
        if isCollision
            continue;
        end

        % 计算到车辆四个方向的最近障碍物的距离
        [frontDistance, backDistance, leftDistance, rightDistance] = VehicleObstDistance(Pos, endYaw, Vehicle, Configure);

        % 距离若为inf则代表没有障碍物，以地图最大斜线距离的四倍代替
        if frontDistance == inf
            frontDistance = infDistance;
        end
        if backDistance == inf
            backDistance = infDistance;
        end
        if leftDistance == inf
            leftDistance = infDistance;
        end
        if rightDistance == inf
            rightDistance = infDistance;
        end

        % 计算最小的四个方向距离平方和
        obstDistance = sqrt(frontDistance^2+backDistance^2+leftDistance^2+rightDistance^2);

        % 检查距离平方和是否最远或已经到达最大距离
        if obstDistance > maximalObstDistance || obstDistance == 2*infDistance
            maximalObstDistance = obstDistance;
            bestEndYaw = endYaw;
        end
    end
end

% 把弧度x控制在[-pi,pi]
function v = Mod2Pi(x)
    v = rem(x,2*pi); % 求整除x/2pi的余数
    if v < -pi
        v = v+2*pi;
    elseif v > pi
        v = v-2*pi;
    end
end